#include "biquad.h"

	.text                  # Text-Segment
	.globl biquad_inplace  # Export Symbol

# extern void biquad_inplace(
# a0: int nsamples,
# a1: int32_t samplebuff[],
# a2: const struct biquad_params *bqparams
# a3: struct biquad_state *bqstate);
	.ent biquad_inplace
	.set reorder
biquad_inplace:
	li   $t8, BIQUAD_HALF                  # t8 = BIQUAD_HALF
	beq  $a0, $zero, .Lend                 # if (nsamples==0) goto .Lend
	lh   $v0, 0($a2)                       # v0 = bqparams->a1
	lh   $v1, 2($a2)                       # v1 = bqparams->a2
	lh   $t3, 4($a2)                       # t3 = bqparams->b1
	lh   $t4, 6($a2)                       # t4 = bqparams->b2
	lw   $t1, 0($a3)                       # t1 = bqstate->t1
	lw   $t2, 4($a3)                       # t2 = bqstate->t2
.Lsampleloop:
	mthi  $zero                            # HI = 0
	mtlo  $t8                              # LO = BIQUAD_HALF
	lw    $t7, 0($a1)                      # t7 = samplebuff[0]

	# t0 = (BIQUAD_HALF
	#       - t1*bqparams->a1
	#       - t2*bqparams->a2) >> BIQUAD_FRAC_BITS;
	msub  $t1, $v0                         # HI::LO -= t1*bqparams->a1
	msub  $t2, $v1                         # HI::LO -= t2*bqparams->a2
	mflo  $t5                              # t5 = LO
	mfhi  $t6                              # t6 = HI
	srl   $t5, $t5, BIQUAD_FRAC_BITS       # t5 = (unsigned)t5 >> BIQUAD_FRAC_BITS
	sll   $t6, $t6, (32-BIQUAD_FRAC_BITS)  # t6 = t6 << (32-BIQUAD_FRAC_BITS)
	or    $t0, $t5, $t6                    # t0 = t5 | t6

	mthi  $zero                            # HI = 0
	mtlo  $t8                              # LO = BIQUAD_HALF
	addu  $t0, $t0, $t7                    # t0 += t7

	# t7 = (BIQUAD_HALF
	#       + t1*bqparams->b1
	#       + t2*bqparams->b2) >> BIQUAD_FRAC_BITS;
	madd  $t1, $t3                         # HI::LO += t1*bqparams->b1
	madd  $t2, $t4                         # HI::LO += t2*bqparams->b2
	mflo  $t5                              # t5 = LO
	mfhi  $t6                              # t6 = HI
	srl   $t5, $t5, BIQUAD_FRAC_BITS       # t5 = (unsigned)t5 >> BIQUAD_FRAC_BITS
	sll   $t6, $t6, (32-BIQUAD_FRAC_BITS)  # t6 = t6 << (32-BIQUAD_FRAC_BITS)
	or    $t7, $t5, $t6                    # t7 = t5 | t6

	addu  $t7, $t7, $t0                    # t7 += t0
	move  $t2, $t1                         # t2 = t1
	move  $t1, $t0                         # t1 = t0
	addiu $a0, $a0, -1                     # nsamples--
	sw    $t7, 0($a1)                      # samplebuff[0] = t7
	addiu $a1, $a1,  4                     # samplebuff++;
	bne   $a0, $zero, .Lsampleloop         # in case (nsamples!=0) goto .Lsampleloop

	sw    $t1, 0($a3)                      # bqstate->t1 = t1
	sw    $t2, 4($a3)                      # bqstate->t2 = t2
.Lend:
	jr    $ra
	.end biquad_inplace
